
package com.grt192.spot.controller;

import com.grt192.core.EventController;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.spot.actuator.GRTDemoLED;
import com.grt192.spot.mechanism.hauntedHouse.StatusLight;
import com.grt192.spot.sensor.GRTSonar;
import com.iaroc.irobot.IRobotCreate;

/**
 *
 * @author ajc
 */
public class FeedbackDistanceDemo extends EventController implements SensorChangeListener {

    public static final int DISTANCE = 10;
    public static final int SPEED = 100;
    private final StatusLight light;
    private final GRTSonar s;
    private final IRobotCreate irc;

    public FeedbackDistanceDemo(StatusLight light, GRTSonar s, IRobotCreate irc) {
        this.light = light;
        this.s = s;
        s.start();
        this.irc = irc;
        setPrinting(true);
    }

    public void start() {
        super.start();
        light.rawColor(GRTDemoLED.Color.ORANGE);
    }

    public void startListening() {
        s.addSensorChangeListener(this);
    }

    public void stopListening() {
        s.removeSensorChangeListener(this);
    }

    public void sensorStateChanged(SensorEvent e, String key) {
//        if(e.getData("Distance"))
        double distance = e.getData("Distance");
        if (distance < DISTANCE) { //too close: back up
            log("Too close! \t" + distance);
            light.rawColor(GRTDemoLED.Color.RED);
            if (irc != null) {
                irc.driveDirect(-SPEED, -SPEED);

            }
        } else if (distance > DISTANCE) {//too far: get closer
            log("Too far! \t" + distance);
            light.rawColor(GRTDemoLED.Color.GREEN);
            if (irc != null) {
                irc.driveDirect(SPEED, SPEED);
            }
        }
    }
}
